﻿/*
  stepper.c - stepper motor driver: executes motion plans using stepper motors
  Part of Grbl

  Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
  Copyright (c) 2009-2011 Simen Svale Skogsrud

  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "grbl.h"


// Some useful constants.
#define DT_SEGMENT (1.0/(ACCELERATION_TICKS_PER_SECOND*60.0)) // min/segment
#define REQ_MM_INCREMENT_SCALAR 1.25
#define RAMP_ACCEL 0
#define RAMP_CRUISE 1
#define RAMP_DECEL 2
#define RAMP_DECEL_OVERRIDE 3

#define PREP_FLAG_RECALCULATE bit(0)            // 重新计算 segment 微段
#define PREP_FLAG_HOLD_PARTIAL_BLOCK bit(1)
#define PREP_FLAG_PARKING bit(2)
#define PREP_FLAG_DECEL_OVERRIDE bit(3)   // 减速覆盖

// st_block_t 结构体用于保存规划器块中Bresenham算法的执行数据，这些数据会被复制到步进段缓冲区中，以便步进算法执行时使用。
// 这样做可以让已经完全执行的规划器块被丢弃，从而释放空间。
//
// 储规划器块的Bresenham算法执行数据，这些数据用于步进段缓冲区中的段。
// 通常，这个缓冲区部分被使用，但在最坏的情况下，它也不会超过可访问的步进段缓冲区段的数量（SEGMENT_BUFFER_SIZE-1）。
// 注意：这些数据从已经准备好的规划器块中复制而来，这样当步进段缓冲区完全消耗并完成这些块时，规划器块可以被丢弃。
// 此外，AMASS（自适应多轴步进平滑）也会修改这些数据以供其使用。
typedef struct {
  uint32_t steps[N_AXIS];       // 每轴要走的步数
  uint32_t step_event_count;    // 事件数。最大步数
  uint8_t direction_bits;       // 方向位
} st_block_t;

static st_block_t st_block_buffer[SEGMENT_BUFFER_SIZE-1];

// 定义了步进电机算法执行的步进段（即小的、短的直线段）的信息
// 这些段从计划器缓冲区的第一个块中“借出”并按顺序执行。 一旦“借出”，则段缓冲区中的步数不能由计划器修改，
// 而计划器剩余的块步数仍然可以。 
typedef struct {
  uint16_t n_step;           // 该segment的步数，是中断执行次数
  uint16_t cycles_per_tick;  // 算法在每个ISR周期内可以移动的步进距离，就是步进速率
  uint8_t  st_block_index;   // 用于指向存储当前步进段相关信息的 st_block_t 结构体
  uint8_t prescaler;         // 在没有使用 AMASS技术时，用于调整步进算法的定时
} segment_t;
static segment_t segment_buffer[SEGMENT_BUFFER_SIZE];

// Stepper ISR data struct. Contains the running data for the main stepper ISR.
typedef struct {
  // Used by the bresenham line algorithm
  uint32_t counter_x,        // Counter variables for the bresenham line tracer
           counter_y,
           counter_z;
  #ifdef STEP_PULSE_DELAY
    uint8_t step_bits;  // Stores out_bits output to complete the step pulse delay
  #endif

  uint8_t execute_step;     // Flags step execution for each interrupt.
  uint8_t step_pulse_time;  // Step pulse reset time after step rise
  uint8_t step_outbits;         // The next stepping-bits to be output
  uint8_t dir_outbits;
  #ifdef ENABLE_DUAL_AXIS
    uint8_t step_outbits_dual;
    uint8_t dir_outbits_dual;
  #endif
  #ifdef ADAPTIVE_MULTI_AXIS_STEP_SMOOTHING
    uint32_t steps[N_AXIS];
  #endif

  uint16_t step_count;       // Steps remaining in line segment motion
  uint8_t exec_block_index; // Tracks the current st_block index. Change indicates new block.
  st_block_t *exec_block;   // Pointer to the block data for the segment being executed
  segment_t *exec_segment;  // Pointer to the segment being executed
} stepper_t;
static stepper_t st;

// Step segment ring buffer indices
static volatile uint8_t segment_buffer_tail;  // 可用数据位置，队列尾。环形队列头部插入，尾部读出
static uint8_t segment_buffer_head;           // 下一个空位置，队列头
static uint8_t segment_next_head;             // 下个队列头

// Step and direction port invert masks.
static uint8_t step_port_invert_mask; // 对应 bit 位置 1 会反转输出
static uint8_t dir_port_invert_mask;

// Used to avoid ISR nesting of the "Stepper Driver Interrupt". Should never occur though.
static volatile uint8_t busy;

// Pointers for the step segment being prepped from the planner buffer. Accessed only by the
// main program. Pointers may be planning segments or planner blocks ahead of what being executed.
static plan_block_t *pl_block;     // 微步细分的当前规划器块指针
static st_block_t *st_prep_block;  // Pointer to the stepper block data being prepped

// Segment preparation data struct. Contains all the necessary information to compute new segments
// based on the current executing planner block.
// 段准备数据结构。 包含基于当前执行计划器块计算新段所需的所有信息。 
typedef struct {
  uint8_t st_block_index;  // st_block_buffer 索引
  uint8_t recalculate_flag;

  float dt_remainder;     // 由于 dt 时间不能被整除，dt 时间余数
  float steps_remaining;  // 剩余步数
  float step_per_mm;      // 递增轴每毫米步数 step/mm
  float req_mm_increment; // 最大步数轴移动一步的最小距离增量 mm

  uint8_t ramp_type;      // 斜坡类型状态，加速、减速、匀速、三角形梯形斜坡
  float mm_complete;      // 块末端到速度斜坡末端的距离 mm
                          // NOTE: This value must coincide with a step(no mantissa) when converted.
  float current_speed;    // 段缓冲器的当前速度Current speed at the end of the segment buffer (mm/min)
  float maximum_speed;    // 段执行最大速度，不总等于 nominal speed (mm/min)
  float exit_speed;       // 段退出速度 Exit speed of executing block (mm/min)
  float accelerate_until; // 加速斜坡结束后剩余距离(mm) S-S1 
  float decelerate_after; // 从 nominal 减速时的减速距离 mm
} st_prep_t;
// 段准备数据
static st_prep_t prep;


/*    BLOCK VELOCITY PROFILE DEFINITION
          __________________________
         /|                        |\     _________________         ^
        / |                        | \   /|               |\        |
       /  |                        |  \ / |               | \       s
      /   |                        |   |  |               |  \      p
     /    |                        |   |  |               |   \     e
    +-----+------------------------+---+--+---------------+----+    e
    |               BLOCK 1            ^      BLOCK 2          |    d
                                       |
                  time ----->      EXAMPLE: Block 2 entry speed is at max junction velocity

  规划器快缓冲的规划假设恒定的加速度速度剖面并且持续地加入在上面展示出的块节点中。但是规划器仅仅活跃在计算块进入速度上
  用来优化规划速度，但是不会计算块内部速度剖面。当他们被执行时，这些速度剖面是被步进算法临时计算的。
  一共有7种可能的剖面：匀速，匀速减速，加速匀速，加速，减速，梯形，三角形。

                                        maximum_speed (< nominal_speed) ->  +
                    +--------+ <- maximum_speed (= nominal_speed)          /|\
                   /          \                                           / | \
 current_speed -> +            \                                         /  |  + <- exit_speed
                  |             + <- exit_speed                         /   |  |
                  +-------------+                     current_speed -> +----+--+
                   time -->  ^  ^                                           ^  ^
                             |  |                                           |  |
                decelerate_after(in mm)                             decelerate_after(in mm)
                    ^           ^                                           ^  ^
                    |           |                                           |  |
                accelerate_until(in mm)                             accelerate_until(in mm)
  步骤段缓冲区计算执行块速度剖面并跟踪临界参数为步进算法精确跟踪轮廓。 这些临界参数在上面的插图上展示定义。 
*/

// 步进电机状态初始化。循环应该仅仅在st.cycle_start标记使能时开始。
// 启动脚本和限位调用这个函数，但是不会启动循环。
void st_wake_up()
{
  // Enable stepper drivers.
  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); }
  else { STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); }

  // Initialize stepper output bits to ensure first ISR call does not step.
  st.step_outbits = step_port_invert_mask;

    // Set step pulse time. Ad hoc computation from oscilloscope. Uses two's complement.
    st.step_pulse_time = -(((settings.pulse_microseconds-2)*TICKS_PER_MICROSECOND) >> 3);

  // Enable Stepper Driver Interrupt
  TIMSK1 |= (1<<OCIE1A);
}


// Stepper shutdown 步进电机关机
void st_go_idle()
{
  // 禁用步进电机驱动器中断。如果激活，允许步进电机端口重置中断完成。
  TIMSK1 &= ~(1<<OCIE1A); // Disable Timer1 interrupt 禁用定时器1中断
  TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Reset clock to no prescaling.重置时钟为没有预分频
  busy = false;

  // Set stepper driver idle state, disabled or enabled, depending on settings and circumstances.
  // 根据设置和环境设置步进电机驱动器为空闲，禁用，启用。
  bool pin_state = false; // Keep enabled.
  if (((settings.stepper_idle_lock_time != 0xff) || sys_rt_exec_alarm || sys.state == STATE_SLEEP) && sys.state != STATE_HOMING) {
    // Force stepper dwell to lock axes for a defined amount of time to ensure the axes come to a complete
    // stop and not drift from residual inertial forces at the end of the last movement.
    // 强制步进电机停留一段确定的时间来锁定轴，以确保轴完全停止，并在最后一次运动结束时不偏离剩余惯性力
    delay_ms(settings.stepper_idle_lock_time);
    pin_state = true; // Override. Disable steppers. 覆写禁用步进电机
  }
  if (bit_istrue(settings.flags,BITFLAG_INVERT_ST_ENABLE)) { pin_state = !pin_state; } // Apply pin invert.应用引脚反转
  if (pin_state) { STEPPERS_DISABLE_PORT |= (1<<STEPPERS_DISABLE_BIT); }
  else { STEPPERS_DISABLE_PORT &= ~(1<<STEPPERS_DISABLE_BIT); }
}

/*
 步进电机驱动器中断，这个定时器中断是Grbl的主力。Grbl 采用了古老的Bresenham直线算法来管理和精确同步多轴运动。
 不像流行的DDA算法，Bresenham算法不受浮点值取舍误差的影响，并且只需要快速的整数计数器，这意味着低计算开销和
 最大化Arduino能力。但是Bresenham算法的缺点是在某些多轴运动中，非主导轴可能会出现不平滑的不仅脉冲序列或锯齿
 会导致可以听见的奇怪的噪音或震动。这在低频运动（0-5千赫兹）是尤为明显，但是在高频下通常不会导致物理问题，虽然能听见。
 为了改进Bresnham多轴性能，Grbl使用我们称之为自适应多轴步进平滑的算法(AMASS)，顾名思义。
 在较低的步进频率下，AMASS认为地提高Bresenham算法的精度而不影响它固有的精度。
 AMASS根据步骤频率自动调整其分辨率级别,这意味着即使是更低的步频,步骤平滑级别也会增加。
 在算法上，AMASS是通过对每个AMASS级别的Bresenham步数进行简单的位移动来实现的。
 例如：对于级别1步数平滑，我们移动一位Bresenham步数，等于它乘以2，其他州也一样，然后加倍步进电机ISR频率。
 实际上，我们允许非主导的Bresenham轴在中间的ISR周期，而主导轴是每两个ISR周期，而不是传统意义上的每一个ISR周期。
 在级别2上，我们简单的再移动一位，因此非主导轴可以再4个ISR周期的任意一个周期步进，主导轴每4个周期步进,并将其ISR频率放大4倍。等等。
 这实际上消除了Bresenham算法的多轴锯齿问题，并没有显著改变Grbl的性能，但实际上，在所有配置中更有效地利用了未使用的CPU周期。
  不管AMASS级别如何，AMASS都要求始终执行完整的Bresenham步骤，从而保持Bresenham算法的准确性。 
  这意味着对于AMASS Level 2，必须完成所有四个中间步骤，以便始终保持基线Bresenham (Level 0)计数。类似地，AMASS Level 3意味着必须执行所有八个中间步骤。  
  虽然AMASS级别实际上是任意的，在这里基线Bresenham计数可以乘以任何整数值，但通过位移位整型运算，2的幂乘只是简单地用于减少CPU开销。

  这个中断在设计上是简单而愚蠢的。所有计算上的繁重工作，如确定加速度，都在其他地方进行。 
  这个中断从步进段缓冲器中弹出预先计算的段，定义为在n个步长内以恒定速度运行，然后通过Bresenham算法脉冲步进相应的管脚来执行它们。
  该ISR支持通过步进端口重置中断，用于在每个脉冲后重置步进端口。Bresenham线跟踪算法使用两个中断同时控制所有步进电机输出。

  注意:这个中断必须尽可能高效，在下一个ISR tick之前完成，这对于Grbl来说必须小于33.3微秒 (@30kHz ISR速率)。
  示波器在ISR中测量的时间典型值是5微秒和最大25微秒，远远低于要求。 
  注意:这个ISR期望每个段至少执行一个步骤。

  代办：以某种方式替换ISR中int32位置计数器的直接更新。 也许只在段完成时使用更小的int8变量和更新位置计数器。  
  这可能会变得复杂，因为探测和归位周期需要真正的实时位置。
*/

// 定时器比较中断中先输出上次设置的脉冲，再计算每轴下个输出
// TCNT0：定时器计数器
// OCR1A：输出比较寄存器
ISR(TIMER1_COMPA_vect)
{
  if (busy) { return; } // The busy-flag is used to avoid reentering this interrupt 这个busy标记用来阻止重新进入这个中断 

  // 在驱动步进电机之前几纳秒设置引脚方向。
  DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | (st.dir_outbits & DIRECTION_MASK);

  // 然后输出脉冲到步进引脚
  STEP_PORT = (STEP_PORT & ~STEP_MASK) | st.step_outbits;

  // 启用步进脉冲重置定时器以便步进电机端口重置中断可以在精确的脉冲时间（微秒）后重置信号，独立于主定时器1预分频器。
  TCNT0 = st.step_pulse_time; //  重加载定时器0计数器
  TCCR0B = (1<<CS01); // Begin Timer0. Full speed, 1/8 prescaler 开始定时器0.全速，1/8预分频

  busy = true;
  sei(); 
  // 重新启用总中断以允许步进端口重置中断及时触发。
  // 注意：这里ISR剩下的代码将会在返回主程序前完成。

  // 如果没有步进段，尝试从步进缓冲器弹出一个。
  if (st.exec_segment == NULL) {
    // 缓冲区里有什么吗? 如果有，加载并初始化下一个步骤段。
    if (segment_buffer_head != segment_buffer_tail) {
      // 初始化一个新的步骤段并加载要执行的步进。
      st.exec_segment = &segment_buffer[segment_buffer_tail];

      // 在禁用AMASS的情况下，为慢频率(< 250Hz)的段设置定时器预分频器。  
      TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (st.exec_segment->prescaler<<CS10);

      // 初始化每个步进的步进段计时，并加载要执行的步骤数。
      OCR1A = st.exec_segment->cycles_per_tick;
      // 注意：当移动缓慢时有可能为0
      st.step_count = st.exec_segment->n_step; // NOTE: Can sometimes be zero when moving slow.
      // 如果新段启动一个新规划器块，初始化步进电机变量和计数器
      // 注意:当段数据索引更改时，这表示一个新的规划器块。  
      if ( st.exec_block_index != st.exec_segment->st_block_index ) {
        st.exec_block_index = st.exec_segment->st_block_index;
        st.exec_block = &st_block_buffer[st.exec_block_index];

        // 初始化Bresenham线和距离计数器
        st.counter_x = st.counter_y = st.counter_z = (st.exec_block->step_event_count >> 1);
      }
      st.dir_outbits = st.exec_block->direction_bits ^ dir_port_invert_mask;  // 方向输出
    } else {
      // 段缓冲器空。关闭电机。
      st_go_idle();
      // 将主程序标记为周期结束
      system_set_exec_state_flag(EXEC_CYCLE_STOP); // Flag main program for cycle end
      return; // Nothing to do but exit.只能退出
    }
  }


  // 检查探针状态。在激活边沿用于获取当前位置
  if (sys_probe_state == PROBE_ACTIVE) { probe_state_monitor(); }

  // Reset step out bits.重置步进输出位
  st.step_outbits = 0;

  // 采用Bresenham线算法执行步进位移破面。
  st.counter_x += st.exec_block->steps[X_AXIS];
  if (st.counter_x > st.exec_block->step_event_count) {
    st.step_outbits |= (1<<X_STEP_BIT);
    st.counter_x -= st.exec_block->step_event_count;

    // 更新系统位置
    if (st.exec_block->direction_bits & (1<<X_DIRECTION_BIT)) { sys_position[X_AXIS]--; }
    else { sys_position[X_AXIS]++; }
  }

  st.counter_y += st.exec_block->steps[Y_AXIS];
  if (st.counter_y > st.exec_block->step_event_count) {
    st.step_outbits |= (1<<Y_STEP_BIT);
    st.counter_y -= st.exec_block->step_event_count;
    if (st.exec_block->direction_bits & (1<<Y_DIRECTION_BIT)) { sys_position[Y_AXIS]--; }
    else { sys_position[Y_AXIS]++; }
  }

  st.counter_z += st.exec_block->steps[Z_AXIS];
  if (st.counter_z > st.exec_block->step_event_count) {
    st.step_outbits |= (1<<Z_STEP_BIT);
    st.counter_z -= st.exec_block->step_event_count;
    if (st.exec_block->direction_bits & (1<<Z_DIRECTION_BIT)) { sys_position[Z_AXIS]--; }
    else { sys_position[Z_AXIS]++; }
  }

  // 在归位周期中，锁定并防止所需的轴移动。 
  // During a homing cycle, lock out and prevent desired axes from moving.
  if (sys.state == STATE_HOMING) { 
    st.step_outbits &= sys.homing_axis_lock;
  }

  st.step_count--; // Decrement step events count 递减步进事件计数。
  if (st.step_count == 0) {
    // 段完成。丢弃当前段并推进段索引
    st.exec_segment = NULL;
    if ( ++segment_buffer_tail == SEGMENT_BUFFER_SIZE) { segment_buffer_tail = 0; }
  }

  st.step_outbits ^= step_port_invert_mask;  // 步进端口反转掩码
  busy = false;
}

/* 步进端口重置中断：定时器0 OVF中断处理步进脉冲的下降沿。这应该在下一个定时器1 COMPA中断之前始终触发，
  并且如果定时器1在完成移动后禁用，则应独立完成。
   注意：如果串行和步进中断在彼此之前执行，它们之间的中断冲突可能会导致几微秒的延迟。这不是什么大问题，
   但如果给Grbl添加另一个高频异步中断，可能会在高步进速率下引起问题。
*/
// 此中断由ISR_TIMER1_COMPAREA启用，当它设置电机端口位以执行步进时。此ISR在短时间后（settings.pulse_microseconds）重置电机端口，完成一个步进周期。

ISR(TIMER0_OVF_vect)
{
  // Reset stepping pins (leave the direction pins)
  STEP_PORT = (STEP_PORT & ~STEP_MASK) | (step_port_invert_mask & STEP_MASK);
  TCCR0B = 0; // Disable Timer0 to prevent re-entering this interrupt when it's not needed.
}
#ifdef STEP_PULSE_DELAY
  // This interrupt is used only when STEP_PULSE_DELAY is enabled. Here, the step pulse is
  // initiated after the STEP_PULSE_DELAY time period has elapsed. The ISR TIMER2_OVF interrupt
  // will then trigger after the appropriate settings.pulse_microseconds, as in normal operation.
  // The new timing between direction, step pulse, and step complete events are setup in the
  // st_wake_up() routine.
  ISR(TIMER0_COMPA_vect)
  {
    STEP_PORT = st.step_bits; // Begin step pulse.
    #ifdef ENABLE_DUAL_AXIS
      STEP_PORT_DUAL = st.step_bits_dual;
    #endif
  }
#endif


// Generates the step and direction port invert masks used in the Stepper Interrupt Driver.
void st_generate_step_dir_invert_masks()
{
  uint8_t idx;
  step_port_invert_mask = 0;
  dir_port_invert_mask = 0;
  for (idx=0; idx<N_AXIS; idx++) {
    if (bit_istrue(settings.step_invert_mask,bit(idx))) { step_port_invert_mask |= get_step_pin_mask(idx); }
    if (bit_istrue(settings.dir_invert_mask,bit(idx))) { dir_port_invert_mask |= get_direction_pin_mask(idx); }
  }
  #ifdef ENABLE_DUAL_AXIS
    step_port_invert_mask_dual = 0;
    dir_port_invert_mask_dual = 0;
    // NOTE: Dual axis invert uses the N_AXIS bit to set step and direction invert pins.    
    if (bit_istrue(settings.step_invert_mask,bit(N_AXIS))) { step_port_invert_mask_dual = (1<<DUAL_STEP_BIT); }
    if (bit_istrue(settings.dir_invert_mask,bit(N_AXIS))) { dir_port_invert_mask_dual = (1<<DUAL_DIRECTION_BIT); }
  #endif
}


// Reset and clear stepper subsystem variables
void st_reset()
{
  // Initialize stepper driver idle state.
  st_go_idle();

  // Initialize stepper algorithm variables.
  memset(&prep, 0, sizeof(st_prep_t));
  memset(&st, 0, sizeof(stepper_t));
  st.exec_segment = NULL;
  pl_block = NULL;  // Planner block pointer used by segment buffer
  segment_buffer_tail = 0;
  segment_buffer_head = 0; // empty = tail
  segment_next_head = 1;
  busy = false;

  st_generate_step_dir_invert_masks();
  st.dir_outbits = dir_port_invert_mask; // Initialize direction bits to default.

  // Initialize step and direction port pins.
  STEP_PORT = (STEP_PORT & ~STEP_MASK) | step_port_invert_mask;
  DIRECTION_PORT = (DIRECTION_PORT & ~DIRECTION_MASK) | dir_port_invert_mask;
  
  #ifdef ENABLE_DUAL_AXIS
    st.dir_outbits_dual = dir_port_invert_mask_dual;
    STEP_PORT_DUAL = (STEP_PORT_DUAL & ~STEP_MASK_DUAL) | step_port_invert_mask_dual;
    DIRECTION_PORT_DUAL = (DIRECTION_PORT_DUAL & ~DIRECTION_MASK_DUAL) | dir_port_invert_mask_dual;
  #endif
}


// Initialize and start the stepper motor subsystem
void stepper_init()
{
  // Configure step and direction interface pins
  STEP_DDR |= STEP_MASK;
  STEPPERS_DISABLE_DDR |= 1<<STEPPERS_DISABLE_BIT;
  DIRECTION_DDR |= DIRECTION_MASK;
  
  #ifdef ENABLE_DUAL_AXIS
    STEP_DDR_DUAL |= STEP_MASK_DUAL;
    DIRECTION_DDR_DUAL |= DIRECTION_MASK_DUAL;
  #endif

  // Configure Timer 1: Stepper Driver Interrupt
  TCCR1B &= ~(1<<WGM13); // waveform generation = 0100 = CTC
  TCCR1B |=  (1<<WGM12);
  TCCR1A &= ~((1<<WGM11) | (1<<WGM10));
  TCCR1A &= ~((1<<COM1A1) | (1<<COM1A0) | (1<<COM1B1) | (1<<COM1B0)); // Disconnect OC1 output
  // TCCR1B = (TCCR1B & ~((1<<CS12) | (1<<CS11))) | (1<<CS10); // Set in st_go_idle().
  // TIMSK1 &= ~(1<<OCIE1A);  // Set in st_go_idle().

  // Configure Timer 0: Stepper Port Reset Interrupt
  TIMSK0 &= ~((1<<OCIE0B) | (1<<OCIE0A) | (1<<TOIE0)); // Disconnect OC0 outputs and OVF interrupt.
  TCCR0A = 0; // Normal operation
  TCCR0B = 0; // Disable Timer0 until needed
  TIMSK0 |= (1<<TOIE0); // Enable Timer0 overflow interrupt
  #ifdef STEP_PULSE_DELAY
    TIMSK0 |= (1<<OCIE0A); // Enable Timer0 Compare Match A interrupt
  #endif
}


// 由planner_recalculate()在执行块由新计划更新时调用。
// 在 st_prep_buffer() 中以当前速度重新计算速度轮廓曲线，并进行步进微段细分
//
void st_update_plan_block_parameters()
{
  if (pl_block != NULL) {                               // 当前已经在计算着当前规划器块，则重新规划计算
    prep.recalculate_flag |= PREP_FLAG_RECALCULATE;     // 重新计算微段
    pl_block->entry_speed_sqr = prep.current_speed*prep.current_speed; // 以当前微段速度作为规划器段入口速度
    pl_block = NULL; // 重新加载并规划当前段
  }
}


// Increments the step segment buffer block data ring buffer.
static uint8_t st_next_block_index(uint8_t block_index)
{
  block_index++;
  if ( block_index == (SEGMENT_BUFFER_SIZE-1) ) { return(0); }
  return(block_index);
}


#ifdef PARKING_ENABLE
  // Changes the run state of the step segment buffer to execute the special parking motion.
  void st_parking_setup_buffer()
  {
    // Store step execution data of partially completed block, if necessary.
    if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) {
      prep.last_st_block_index = prep.st_block_index;
      prep.last_steps_remaining = prep.steps_remaining;
      prep.last_dt_remainder = prep.dt_remainder;
      prep.last_step_per_mm = prep.step_per_mm;
    }
    // Set flags to execute a parking motion
    prep.recalculate_flag |= PREP_FLAG_PARKING;
    prep.recalculate_flag &= ~(PREP_FLAG_RECALCULATE);
    pl_block = NULL; // Always reset parking motion to reload new block.
  }


  // Restores the step segment buffer to the normal run state after a parking motion.
  void st_parking_restore_buffer()
  {
    // Restore step execution data and flags of partially completed block, if necessary.
    if (prep.recalculate_flag & PREP_FLAG_HOLD_PARTIAL_BLOCK) {
      st_prep_block = &st_block_buffer[prep.last_st_block_index];
      prep.st_block_index = prep.last_st_block_index;
      prep.steps_remaining = prep.last_steps_remaining;
      prep.dt_remainder = prep.last_dt_remainder;
      prep.step_per_mm = prep.last_step_per_mm;
      prep.recalculate_flag = (PREP_FLAG_HOLD_PARTIAL_BLOCK | PREP_FLAG_RECALCULATE);
      prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm; // Recompute this value.
    } else {
      prep.recalculate_flag = false;
    }
    pl_block = NULL; // Set to reload next block.
  }
#endif


/* Prepares step segment buffer. Continuously called from main program.

  准备步进段缓冲。被主程序持续调用。

  段缓冲区是步进算法执行步进和规划器生成的速度剖面之间的中间缓冲区接口。 
  步进算法只在段缓冲区内执行步骤，当步骤从规划器缓冲区中的第一个块“检出”时，主程序将填充这些步骤。 
  这使步骤执行和计划优化流程保持原子性，并相互保护。  
  从规划器缓冲区“签出”的步骤数和段缓冲区中的段数的大小和计算，使主程序中的任何操作所花费的时间都不超过步进算法在填充它之前清空它所花费的时间。 
  目前，段缓冲区保守地保存大约40-50毫秒的步长。 
  注意:计算单位为步、毫米和分钟。   

*/
void st_prep_buffer()
{
  // 当处于挂起状态且没有挂起动作要执行时，阻塞步骤准备缓冲区。
  if (bit_istrue(sys.step_control,STEP_CONTROL_END_MOTION)) { return; }

  while (segment_buffer_tail != segment_next_head) { // Check if we need to fill the buffer. 检查是否需要填充缓冲区

    // 确定是否需要加载一个新的规划器块，或者是否需要重新计算该块。
    if (pl_block == NULL) {

      // Query planner for a queued block 为队列块查询规划器
      if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) { pl_block = plan_get_system_motion_block(); }
      else { pl_block = plan_get_current_block(); }
      if (pl_block == NULL) { return; } // No planner blocks. Exit. 没有规划器块，退出。

      // 检查是否需要重新计算 segment 微段 
      if (prep.recalculate_flag & PREP_FLAG_RECALCULATE) {
          prep.recalculate_flag = false;
      } else {
        // 加载块的Bresenham步进数据。  
        prep.st_block_index = st_next_block_index(prep.st_block_index);

        // 从新的规划器块中准备并复制Bresenham算法的段数据，以便当段缓冲区完成规划器块时，当段缓冲区完成准备块时，它可能被丢弃，但步进器ISR仍在执行它。  
        st_prep_block = &st_block_buffer[prep.st_block_index];
        st_prep_block->direction_bits = pl_block->direction_bits;

        uint8_t idx;
        for (idx=0; idx<N_AXIS; idx++) { st_prep_block->steps[idx] = (pl_block->steps[idx] << 1); }
        st_prep_block->step_event_count = (pl_block->step_event_count << 1);

        // 初始化段缓冲区数据以生成段。保证按最长距离轴每次移动至少一步
        prep.steps_remaining = (float)pl_block->step_event_count;
        prep.step_per_mm = prep.steps_remaining/pl_block->millimeters;
        prep.req_mm_increment = REQ_MM_INCREMENT_SCALAR/prep.step_per_mm;
        prep.dt_remainder = 0.0; // Reset for new segment block 重置新的段块

        if ((sys.step_control & STEP_CONTROL_EXECUTE_HOLD) || (prep.recalculate_flag & PREP_FLAG_DECEL_OVERRIDE)) {
          // 新区块中途加载。 覆写计划器块进入速度以强制减速。 
          prep.current_speed = prep.exit_speed;
          pl_block->entry_speed_sqr = prep.exit_speed*prep.exit_speed;
          prep.recalculate_flag &= ~(PREP_FLAG_DECEL_OVERRIDE);
        } else {
          prep.current_speed = sqrt(pl_block->entry_speed_sqr);
        }
      }

    /* ---------------------------------------------------------------------------------
        根据新的规划块的进入和退出速度计算其速度剖面图，或者重新计算已部分完成的规划块的剖面图(如果规划器更新了它)。  
        对于一个命令的强制减速，例如从一个进给保持，覆盖计划速度和减速到目标出口速度。
    */
    prep.mm_complete = 0.0; // 默认速度剖面到块结束时速度为0
    float inv_2_accel = 0.5/pl_block->acceleration; // 1/(2a)
    if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) { // 每次强制减速到0
      // Compute velocity profile parameters for a feed hold in-progress. This profile overrides
      // the planner block profile, enforcing a deceleration to zero speed.
      prep.ramp_type = RAMP_DECEL;
      // Compute decelerate distance relative to end of block.
      float decel_dist = pl_block->millimeters - inv_2_accel*pl_block->entry_speed_sqr;
      if (decel_dist < 0.0) {
          // Deceleration through entire planner block. End of feed hold is not in this block.
          prep.exit_speed = sqrt(pl_block->entry_speed_sqr-2*pl_block->acceleration*pl_block->millimeters);
      } else {
          prep.mm_complete = decel_dist; // End of feed hold.
          prep.exit_speed = 0.0;
      }
    } else { // [Normal Operation] 常规操作
      // Compute or recompute velocity profile parameters of the prepped planner block.
      prep.ramp_type = RAMP_ACCEL; // 初始化为加速斜线
      prep.accelerate_until = pl_block->millimeters; // 加速到的位置

      float exit_speed_sqr;   // 退出速度的平方
      float nominal_speed;    // 标称速度
      if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
        prep.exit_speed = exit_speed_sqr = 0.0; // Enforce stop at end of system motion.强制在系统运动结束时停止。
      } else {
        exit_speed_sqr = plan_get_exec_block_exit_speed_sqr();
        prep.exit_speed = sqrt(exit_speed_sqr);
      }

        nominal_speed = plan_compute_profile_nominal_speed(pl_block); // 当前运动控制块的标称速度
        float nominal_speed_sqr = nominal_speed*nominal_speed;        // 标称速度的平方
        // 计算速度曲线中的交点距离。整个 S 长线段中，减速段运行的距离
        float intersect_distance =
                   0.5*(pl_block->millimeters+inv_2_accel*(pl_block->entry_speed_sqr-exit_speed_sqr));

        if (pl_block->entry_speed_sqr > nominal_speed_sqr) { // 在减速覆盖时，入口速度比运行速度大
          prep.accelerate_until = pl_block->millimeters - inv_2_accel*(pl_block->entry_speed_sqr-nominal_speed_sqr);
          if (prep.accelerate_until <= 0.0) { // Deceleration-only. 只有减速
            prep.ramp_type = RAMP_DECEL;
            // prep.decelerate_after = pl_block->millimeters;
            // prep.maximum_speed = prep.current_speed;

            // 计算覆盖块退出速度，因为它不匹配规划器退出速度。  
            prep.exit_speed = sqrt(pl_block->entry_speed_sqr - 2*pl_block->acceleration*pl_block->millimeters);
            prep.recalculate_flag |= PREP_FLAG_DECEL_OVERRIDE; // Flag to load next block as deceleration override.

            // TODO: Determine correct handling of parameters in deceleration-only.
            // Can be tricky since entry speed will be current speed, as in feed holds.
            // Also, look into near-zero speed handling issues with this.

          } else {
            // 减速到匀速或匀减速,保证与更新的计划相交。
            // Decelerate to cruise or cruise-decelerate types. Guaranteed to intersect updated plan.
            prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr); // Should always be >= 0.0 due to planner reinit.
            prep.maximum_speed = nominal_speed;
            prep.ramp_type = RAMP_DECEL_OVERRIDE;
            }
        } else if (intersect_distance > 0.0) {
          if (intersect_distance < pl_block->millimeters) { //  梯形或三角形类型
            // NOTE: For acceleration-cruise and cruise-only types, following calculation will be 0.0.
            // 注意：为了加速-巡航和巡航类型，下面的计算将会变成0.0.
            prep.decelerate_after = inv_2_accel*(nominal_speed_sqr-exit_speed_sqr);
            if (prep.decelerate_after < intersect_distance) { // Trapezoid type 梯形轮廓
                prep.maximum_speed = nominal_speed;
                if (pl_block->entry_speed_sqr == nominal_speed_sqr) {
                    // 入口速度等于正常速度，进入匀速状态
                    prep.ramp_type = RAMP_CRUISE;
                } else {
                    // Full-trapezoid or acceleration-cruise types
                    // 进入加速状态
                    prep.accelerate_until -= inv_2_accel*(nominal_speed_sqr-pl_block->entry_speed_sqr);
                }
            } else { // Triangle type 三角形剖面
                prep.accelerate_until = intersect_distance;
                prep.decelerate_after = intersect_distance;
                prep.maximum_speed = sqrt(2.0*pl_block->acceleration*intersect_distance+exit_speed_sqr);  // vm 三角形顶点最大速度
            }
         } else { // Deceleration-only type 只减速剖面
            prep.ramp_type = RAMP_DECEL;
            // prep.decelerate_after = pl_block->millimeters;
            // prep.maximum_speed = prep.current_speed;
          }
        } else { // Acceleration-only type
        prep.accelerate_until = 0.0;
        // prep.decelerate_after = 0.0;
        prep.maximum_speed = prep.exit_speed;
        }
       }
    }
    
    // 初始化一个新段
    segment_t *prep_segment = &segment_buffer[segment_buffer_head];

    // 设置一个新的段指向当前段数据块
    prep_segment->st_block_index = prep.st_block_index;

    /*------------------------------------------------------------------------------------
      通过确定在分段时间DT_SEGMENT上所走过的总距离来计算这个新分段的平均速度。  
      下面的代码首先尝试基于当前的斜坡条件创建一个完整的分段。
      如果在斜坡状态更改时终止时段时间不完整，则代码将继续循环遍历进行中的斜坡状态以填充剩余的段执行时间。
      但是，如果不完整的段在速度曲线的末尾终止，则尽管截断的执行时间小于 DT_SEGMENT，但仍认为该段已完成。

      速度曲线始终假定通过斜坡序列进行：加速斜坡、巡航状态和减速斜坡。
      每个斜坡的行进距离的范围可以从零到块的长度。
      速度剖面可以在规划块的末端(典型的)或强制减速的中间块的末端结束，例如从进给保持。 
    */
    // 使用增量法以 dt 为单位离散和计算速度及位移
    float dt_max = DT_SEGMENT; // Maximum segment time 最大的段时间
    float dt = 0.0; // 当前微段占用的时间
    float time_var = dt_max; // 当前工作时间
    float mm_var; // mm-Distance worker variable 毫米距离工人变量
    float speed_var; // Speed worker variable 速度工人变量
    float mm_remaining = pl_block->millimeters; // 轨迹块结束剩余距离 mm
    float minimum_mm = mm_remaining-prep.req_mm_increment; // 移动最小步进位置后剩余距离
    if (minimum_mm < 0.0) { minimum_mm = 0.0; }

    do {
      switch (prep.ramp_type) {
        case RAMP_DECEL_OVERRIDE:   // 减速覆盖
          speed_var = pl_block->acceleration*time_var;
          if (prep.current_speed-prep.maximum_speed <= speed_var) {
            // Cruise or cruise-deceleration types only for deceleration override.
            mm_remaining = prep.accelerate_until;
            time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);
            prep.ramp_type = RAMP_CRUISE;
            prep.current_speed = prep.maximum_speed;
          } else { // Mid-deceleration override ramp.
            mm_remaining -= time_var*(prep.current_speed - 0.5*speed_var);
            prep.current_speed -= speed_var;
          }
          break;
        case RAMP_ACCEL:    // 加速
          // 计算加速 dt 运行的距离
          // s1=(0.5*(pl_block->acceleration*time_var)+prep.current_speed)*dt_max
          speed_var = pl_block->acceleration*time_var;
          mm_remaining -= time_var*(prep.current_speed + 0.5*speed_var);    // 剩余距离减 dt 时间移动距离
          if (mm_remaining < prep.accelerate_until) {     // 加速斜坡结束
            // 加速到匀速或加速到减速的交界处，根据实际距离重新修正 DT 实际
            mm_remaining = prep.accelerate_until;         // 加速段完成后剩余位置
            // 真实时间 dt = ds/((v+vm)/2)
            time_var = 2.0*(pl_block->millimeters-mm_remaining)/(prep.current_speed+prep.maximum_speed);

            // 判断进入匀速还是减速
            if (mm_remaining == prep.decelerate_after) { prep.ramp_type = RAMP_DECEL; }
            else { prep.ramp_type = RAMP_CRUISE; }
            prep.current_speed = prep.maximum_speed;
          } else {
            prep.current_speed += speed_var;    // 加速，速度递增
          }
          break;
        case RAMP_CRUISE:   // 匀速
          // NOTE: mm_var used to retain the last mm_remaining for incomplete segment time_var calculations.
          // NOTE: If maximum_speed*time_var value is too low, round-off can cause mm_var to not change. To
          //   prevent this, simply enforce a minimum speed threshold in the planner.
          mm_var = mm_remaining - prep.maximum_speed*time_var;
          if (mm_var < prep.decelerate_after) { // End of cruise.
            // Cruise-deceleration junction or end of block.
            time_var = (mm_remaining - prep.decelerate_after)/prep.maximum_speed;
            mm_remaining = prep.decelerate_after; // NOTE: 0.0 at EOB
            prep.ramp_type = RAMP_DECEL;
          } else { // Cruising only.
            mm_remaining = mm_var;
          }
          break;
        default: // case RAMP_DECEL: 默认减速
          // NOTE: mm_var used as a misc worker variable to prevent errors when near zero speed.
          speed_var = pl_block->acceleration*time_var; // Used as delta speed (mm/min)
          if (prep.current_speed > speed_var) { // Check if at or below zero speed.
            // Compute distance from end of segment to end of block.
            mm_var = mm_remaining - time_var*(prep.current_speed - 0.5*speed_var); // (mm)
            if (mm_var > prep.mm_complete) { // Typical case. In deceleration ramp.
              mm_remaining = mm_var;
              prep.current_speed -= speed_var;
              break; // Segment complete. Exit switch-case statement. Continue do-while loop.
            }
          }
          
          // 块结束或强制减速结束
          time_var = 2.0*(mm_remaining-prep.mm_complete)/(prep.current_speed+prep.exit_speed);
          mm_remaining = prep.mm_complete;
          prep.current_speed = prep.exit_speed;
      }

      // 段所用的时间，DT 时间超过 DT_SEGMENT 并且至少移动一步时退出循环
      dt += time_var; // Add computed ramp time to total segment time.
      if (dt < dt_max) { time_var = dt_max - dt; } // 在斜坡连接点，不凑整。计算余数 **Incomplete** At ramp junction.
      else {
        // 没有移动最小步进位置 prep.req_mm_increment，递增下个 DT_SEGMENT 时间片
        if (mm_remaining > minimum_mm) {
          // Increase segment time to ensure at least one step in segment. Override and loop
          // through distance calculations until minimum_mm or mm_complete.
          dt_max += DT_SEGMENT;       // 占用多个 DT 时间片
          time_var = dt_max - dt;     // 下个时间位置
        } else {
          break; // 完成退出循环**Complete** Exit loop. Segment execution time maxed.
        }
      }
    } while (mm_remaining > prep.mm_complete); // **Complete** Exit loop. Profile complete.
    
    /* -----------------------------------------------------------------------------------
      计算步进段的步进速率、要执行的步数，并应用必要的速率校正。
      注意：步数是通过直接将块中剩余的毫米距离转换为标量来计算的，而不是逐步累加每个段
      执行的步数。这有助于消除多次加法导致的浮点数舍入误差问题。然而，由于浮点数只
      有7.2位有效数字，因此非常长的移动和极高的步数可能会超出浮点数的精度，这可能导致
      步数丢失。幸运的是，在Grbl支持（即每毫米200步，超过10米轴行程）的CNC机床上，这种
      情况几乎不可能发生，也不切实际。
    */
    float step_dist_remaining = prep.step_per_mm*mm_remaining; // 剩余距离转换为递增轴步数
    float n_steps_remaining = ceil(step_dist_remaining);       // 剩余步数向上取整 Round-up current steps remaining
    float last_n_steps_remaining = ceil(prep.steps_remaining); // 前一次剩余步数向上取整 Round-up last steps remaining
    prep_segment->n_step = last_n_steps_remaining-n_steps_remaining; // 前一次剩余步数-现在剩余步数，就是当前需要走的步数

    // Bail if we are at the end of a feed hold and don't have a step to execute.
    if (prep_segment->n_step == 0) {
      if (sys.step_control & STEP_CONTROL_EXECUTE_HOLD) {
        // Less than one step to decelerate to zero speed, but already very close. AMASS
        // requires full steps to execute. So, just bail.
        bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
        #ifdef PARKING_ENABLE
          if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; }
        #endif
        return; // Segment not generated, but current step data still retained.
      }
    }
    

    // 计算步进段的步进速率。由于步数是整数，而移动的毫米距离不是整数，
    // 因此每个段的末尾可能会有不同幅度的部分步进，这些部分步进不会被执行，
    // 因为步进中断服务程序（ISR）由于AMASS算法的要求需要整步。为了补偿这一点，
    // 我们跟踪执行前一个段的部分步进的时间，并将其与部分步进距离一起简单地应用到当前段，
    // 这样可以微小地调整整个段的速率，以保持步进输出精确。这些速率调整通常非常小，
    // 不会对性能产生不利影响，但确保Grbl输出由规划器计算出的精确的加速度和速度曲线。
    dt += prep.dt_remainder; // Apply previous segment partial step execute time
    float inv_rate = dt/(last_n_steps_remaining - step_dist_remaining); // Compute adjusted step rate inverse

    // Compute CPU cycles per step for the prepped segment.
    // 根据每步时间，计算分频和周期值
    uint32_t cycles = ceil( (TICKS_PER_MICROSECOND*1000000*60)*inv_rate ); // (cycles/step)

    // Compute step timing and timer prescalar for normal step generation.
    if (cycles < (1UL << 16)) { // < 65536  (4.1ms @ 16MHz)
      prep_segment->prescaler = 1; // prescaler: 0
      prep_segment->cycles_per_tick = cycles;
    } else if (cycles < (1UL << 19)) { // < 524288 (32.8ms@16MHz)
      prep_segment->prescaler = 2; // prescaler: 8
      prep_segment->cycles_per_tick = cycles >> 3;
    } else {
      prep_segment->prescaler = 3; // prescaler: 64
      if (cycles < (1UL << 22)) { // < 4194304 (262ms@16MHz)
        prep_segment->cycles_per_tick =  cycles >> 6;
      } else { // Just set the slowest speed possible. (Around 4 step/sec.)
        prep_segment->cycles_per_tick = 0xffff;
      }
    }

    // Segment 处理完成。递增段索引，这样步进中断可以立即使用
    segment_buffer_head = segment_next_head;
    if ( ++segment_next_head == SEGMENT_BUFFER_SIZE ) { segment_next_head = 0; }

    // Update the appropriate planner and segment data.
    pl_block->millimeters = mm_remaining;
    prep.steps_remaining = n_steps_remaining;
    prep.dt_remainder = (n_steps_remaining - step_dist_remaining)*inv_rate;

    // Check for exit conditions and flag to load next planner block.
    if (mm_remaining == prep.mm_complete) {
      // End of planner block or forced-termination. No more distance to be executed.
      if (mm_remaining > 0.0) { // At end of forced-termination.
        // Reset prep parameters for resuming and then bail. Allow the stepper ISR to complete
        // the segment queue, where realtime protocol will set new state upon receiving the
        // cycle stop flag from the ISR. Prep_segment is blocked until then.
        bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
        #ifdef PARKING_ENABLE
          if (!(prep.recalculate_flag & PREP_FLAG_PARKING)) { prep.recalculate_flag |= PREP_FLAG_HOLD_PARTIAL_BLOCK; }
        #endif
        return; // Bail!
      } else { // End of planner block
        // The planner block is complete. All steps are set to be executed in the segment buffer.
        if (sys.step_control & STEP_CONTROL_EXECUTE_SYS_MOTION) {
          bit_true(sys.step_control,STEP_CONTROL_END_MOTION);
          return;
        }
        pl_block = NULL; // 规划器块结束，准备处理下个块
        plan_discard_current_block();
      }
    }
  }
}


// Called by realtime status reporting to fetch the current speed being executed. This value
// however is not exactly the current speed, but the speed computed in the last step segment
// in the segment buffer. It will always be behind by up to the number of segment blocks (-1)
// divided by the ACCELERATION TICKS PER SECOND in seconds.
float st_get_realtime_rate()
{
  if (sys.state & (STATE_CYCLE | STATE_HOMING | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)){
    return prep.current_speed;
  }
  return 0.0f;
}
